/*
 * AnglePID.cpp
 *
 *  Created on: 28.12.2012
 *      Author: Michael & Aleix
 */

#include "rodos.h"
#include "AnglePID.h"
#include "../Threads/global.h"
#include "../hardware.h"

#include <stdlib.h>
#include <math.h>
#include "../Threads/global.h"
#include "hal_uart.h"
#include <stdio.h>

extern Systemstate state;
extern PowerDriver motor;

char debugbuffer[100];

AnglePID::AnglePID() {
	pre_error_a = 0;
	integral_a = 0;
	output_a = 0;
	P = 0;
	I = 0;
	D = 0;
	state.k_pAngle = 0.22;
	state.k_iAngle = 0.0001;
	state.k_dAngle = 1.0;
	last = NOW();
	state.eps_a = 5;
}

AnglePID::~AnglePID() {
	// TODO Auto-generated destructor stub
}

void AnglePID::resetPID(){
	pre_error_a = 0;
	integral_a = 0;
	output_a = 0;
	P = 0;
	I = 0;
	D = 0;
}

void AnglePID::run() {


	float error = 0;
	float derivative = 0;
	
	error = state.desAngle - state.angleKalman;
	if (error > 180) {
		error = error - 360;
	} else if(error < -180){
		error = error + 360;
	}

//	xprintf("e = %f\n", error);
//	xprintf("des = %f ", state.desAngle);
//	xprintf("kal = %f \n", state.angleKalman);

//	long long now = NOW();
//	float dt = (now - last) / SECONDS;
//	last = now;

	// Avoid integration for too small errors
	if (fabs(error) > state.eps_a) {
		integral_a += error;  // * SAMPLETIME_S;
	}
	derivative = (error - pre_error_a);  // / SAMPLETIME_S;

	P = state.k_pAngle * error;
	I = state.k_iAngle * integral_a;
	D = state.k_dAngle * derivative;
	output_a = P + I + D;

	xprintf("state.k_pAngle = %f\n",state.k_pAngle);

	// Save the error
	pre_error_a = error;

	if (output_a > LIMIT_VOLTAGE) {
		output_a = LIMIT_VOLTAGE;
	} else if (output_a < -LIMIT_VOLTAGE) {
		output_a = -LIMIT_VOLTAGE;
	}

	state.dutycycle = (float)(output_a / MAX_VOLTAGE * 100);



//	int cnt = sprintf(debugbuffer, "e = %f, out = %f\n", error, controlout);
//	uart2.write(debugbuffer, cnt);
	motor.setSpeedPercentage(state.dutycycle );
}
